package it.gius.pe.algorithm.gjk;

import org.jbox2d.common.Vec2;

import it.gius.pe.SystemCostants;
import it.gius.pe.Shape.Polygon;
import it.gius.pe.Shape.witness.PolyPointWitness;
import it.gius.pe.Shape.witness.Witness;
import it.gius.pe.algorithm.gjk.Simplex.SimplexEdge;

public abstract class GjkEpaAlgorithm {


	private static final GjkUtilsFunctions utils = new GjkUtilsFunctions();
 

	public static final int iterationCoefficient = 8;

	public static void distance(IGjkAlgorithmShape shape,Vec2 q, DistanceSolution sol)
	{
		/*Simplex simplex = new Simplex(Simplex.LARGER_2D_SIMPLEX);
		simplex.vs[0].set(shape.getStartingPoint());
		simplex.currentDim = Simplex.IS_0_SIMPLEX;*/
		Simplex simplex = shape.getStartingSimplex();
		

		SimplexSolution simpSol = new SimplexSolution();
		

		Vec2 d = null;
		Vec2 currSol = new Vec2();
		Vec2 supportPoint = new Vec2();
		SimplexSolution oldSimpSol = new SimplexSolution();

		//float dim = (float)poly.getVerticesCount();
		float dim = (float)shape.getDim();

		//int max = (int)(dim*dim*dim/6 + dim*dim/2 + dim);
		int max = (int)(iterationCoefficient*dim);

		for(int iteration=0; iteration<max; iteration++)
		{

			utils.nearestPointSimplex2D(simplex, q, simpSol);

			simpSol.getSolutionPoint(currSol);

			d = q.sub(currSol); 

			//shape.fillSolution(currSol,q, simpSol, sol);
			
			Witness lastWitness = shape.supportPoint(d, supportPoint);

			//simplex = shape.updateSimplex(simpSol, supportPoint/*d*/,lastWitness);

			float g = utils.g(currSol,q,supportPoint/*shape*/);

			if(simpSol.numVerticesUsed == Simplex.LARGER_2D_SIMPLEX)
			{
				//System.out.println("g: " +g);
				sol.iterationCount = iteration+1;
				sol.distance = 0;
				sol.penetration = true;
				shape.fillSolution(currSol,q, simpSol, sol);
				
				
				SimplexEdge edgeSolution = new SimplexEdge();
				EpaAlgorithm.penetrationNormalAndDepth(shape, simplex, q, edgeSolution);
				sol.penetrationDepth = edgeSolution.distance;
				sol.normalPenetration = edgeSolution.normal;
				sol.epaIterations = edgeSolution.iterations;
				
				return;
			}
			
			if((g >= -SystemCostants.SQRT_EPSILON && g<= SystemCostants.SQRT_EPSILON) ||  oldSimpSol.equals(simpSol))
			{

				//sol.p1 = currSol;
				sol.iterationCount = iteration+1;
				sol.distance = d.length();
				sol.penetration = false;
				shape.fillSolution(currSol,q, simpSol, sol);

				return;
			}

			simplex = shape.updateSimplex(simpSol, supportPoint/*d*/,lastWitness);

			if(simplex == null)
			{
				sol.iterationCount = iteration+1;
				sol.distance = d.length();
				sol.penetration = false;
				shape.fillSolution(currSol,q, simpSol, sol);
				return;				
			}

			oldSimpSol.set(simpSol);

		}

		throw new NotConvergingException();
	}


	public static void distance(Polygon poly,Vec2 q,DistanceSolution sol) throws NotConvergingException
	{
		Simplex simplex = new Simplex(Simplex.LARGER_2D_SIMPLEX);
		simplex.vs[0].set(poly.getVertex(0));
		simplex.currentDim = Simplex.IS_0_SIMPLEX;

		SimplexSolution npSol = new SimplexSolution();

		Vec2 d = null;
		Vec2 currSol = new Vec2();;

		float dim = (float)poly.getVerticesCount();

		//int max = (int)(dim*dim*dim/6 + dim*dim/2 + dim);
		int max = (int)(dim*dim);

		for(int iteration=0; iteration<max; iteration++)
		{

			utils.nearestPointSimplex2D(simplex, q, npSol);

			npSol.getSolutionPoint(currSol);

			float g = utils.g(currSol,q,poly);

			d = q.sub(currSol); 
			if(g >= -SystemCostants.EPSILON && g<= SystemCostants.EPSILON)
			{
				sol.p1 = currSol;
				sol.distance = d.length();
				return;
			}


			if(npSol.numVerticesUsed == Simplex.LARGER_2D_SIMPLEX)
			{
				sol.p1 = q;
				//TODO distanza negativa tale che  d(Ki,Kj) = -min{|z| : z appartiene a boundary di K}
				sol.distance = 0;
				return;
			}

			int index = utils.supportPointIndex(poly, d);

			boolean supportAlreadyIn = false;

			for(int i =0; i< npSol.numVerticesUsed;i++)
			{
				simplex.vs[i] = npSol.verticesInSolution[i];
				if(simplex.vs[i].equals(poly.getVertex(index)))
					supportAlreadyIn = true;
			}

			simplex.currentDim = npSol.numVerticesUsed;

			if(supportAlreadyIn)
			{
				sol.p1 = currSol;
				sol.distance = d.length();
				return;
			}

			if(simplex.currentDim < Simplex.LARGER_2D_SIMPLEX)
			{
				simplex.vs[npSol.numVerticesUsed] = poly.getVertex(index);
				simplex.currentDim++;
			}

		}

		throw new NotConvergingException();

	}

	public static void distance(Polygon poly1,Polygon poly2,DistanceSolution sol) throws NotConvergingException
	{
		distance(poly1,poly2,sol,0,0);
	}

	public static void distance(Polygon poly1,Polygon poly2,DistanceSolution sol,int startPoint1,int startPoint2) throws NotConvergingException
	{
		Simplex simplex1 = new Simplex(Simplex.LARGER_2D_SIMPLEX);

		if(startPoint1 >=0 && startPoint1<= poly1.getVerticesCount())
		{
			simplex1.vs[0].set(poly1.getVertex(startPoint1));
			PolyPointWitness witness = new PolyPointWitness();
			witness.index = startPoint1;
			simplex1.witnesses[0]/*indicesPoly[0]*/ = witness;
		}
		else
		{
			simplex1.vs[0].set(poly1.getVertex(0));
			PolyPointWitness witness = new PolyPointWitness();
			witness.index = 0;
			simplex1.witnesses[0]/*indicesPoly[0]*/ = witness;
		}

		simplex1.currentDim = Simplex.IS_0_SIMPLEX;

		Simplex simplex1Temp = new Simplex(Simplex.LARGER_2D_SIMPLEX);

		Simplex simplex2 = new Simplex(Simplex.LARGER_2D_SIMPLEX);

		if(startPoint2 >=0 && startPoint2<= poly2.getVerticesCount())
		{
			simplex2.vs[0].set(poly2.getVertex(startPoint2));
			PolyPointWitness witness = new PolyPointWitness();
			witness.index = startPoint2;
			simplex2.witnesses[0]/*indicesPoly[0]*/ = witness;
		}
		else
		{
			simplex2.vs[0].set(poly2.getVertex(0));
			PolyPointWitness witness = new PolyPointWitness();
			witness.index = 0;
			simplex2.witnesses[0]/*indicesPoly[0]*/ = witness;
		}

		simplex2.currentDim = Simplex.IS_0_SIMPLEX;

		Simplex simplex2Temp = new Simplex(Simplex.LARGER_2D_SIMPLEX);

		Simplex simplexZ = new Simplex(Simplex.LARGER_2D_SIMPLEX);
		simplexZ.vs[0] = simplex2.vs[0].sub(simplex1.vs[0]);
		simplexZ.currentDim = Simplex.IS_0_SIMPLEX; 

		SimplexSolution simpSol = new SimplexSolution();

		Vec2 d = null;
		Vec2 currSol = new Vec2();
		Vec2 origin = SystemCostants.ORIGIN2D;

		Vec2 p1Temp,p2Temp;

		float dim = (float)(poly1.getVerticesCount() + poly2.getVerticesCount());

		//int max = (int)(dim*dim*dim/6 + dim*dim/2 + dim);
		int max = (int)(dim*dim);


		for(int iteration=0; iteration<max; iteration++)		
		{

			utils.nearestPointSimplex2D(simplexZ, origin, simpSol);

			simpSol.getSolutionPoint(currSol);

			float g = utils.gOrigin(currSol, poly1, poly2);

			//System.out.println("gorigin: " + g);

			p1Temp = getPointSolution(simplex1, simplexZ, simpSol);
			p2Temp = getPointSolution(simplex2, simplexZ, simpSol);

			d = origin.sub(currSol); 


			int index2 = utils.supportPointIndex(poly2, d);
			int index1 = utils.supportPointIndex(poly1, currSol);

			Vec2 zSupport = poly2.getVertex(index2).sub(poly1.getVertex(index1));

			boolean supportAlreadyIn = false;

			for(int i =0; i< simpSol.numVerticesUsed;i++)
			{
				int j;
				for( j=0; !simplexZ.vs[j].equals(simpSol.verticesInSolution[i]);j++);

				simplex1Temp.vs[i] = simplex1.vs[j];
				simplex1Temp.witnesses[i]/*indicesPoly[i]*/ = simplex1.witnesses[j]/*.clone()/*indicesPoly[j]*/;
				simplex2Temp.vs[i] = simplex2.vs[j];
				simplex2Temp.witnesses[i]/*indicesPoly[i]*/ = simplex2.witnesses[j]/*.clone()/*indicesPoly[j]*/;
				/*if(simplex.vs[i].equals(poly.getVertex(index)))
					supportAlreadyIn = true;*/
			}

			for(int i =0; i< simpSol.numVerticesUsed;i++)
			{
				simplex1.vs[i] = simplex1Temp.vs[i];
				simplex1.witnesses[i]/*indicesPoly[i]*/ = simplex1Temp.witnesses[i]/*indicesPoly[i]*/;
				simplex2.vs[i] = simplex2Temp.vs[i];
				simplex2.witnesses[i]/*indicesPoly[i]*/ = simplex2Temp.witnesses[i]/*indicesPoly[i]*/;

				simplexZ.vs[i] = simplex2.vs[i].sub(simplex1.vs[i]);

				if(simplexZ.vs[i].equals(zSupport))
					supportAlreadyIn = true;
			}

			simplex1.currentDim = simpSol.numVerticesUsed;
			simplex2.currentDim = simpSol.numVerticesUsed;
			simplexZ.currentDim = simpSol.numVerticesUsed;


			if((g >= -SystemCostants.SQRT_EPSILON && g<= SystemCostants.SQRT_EPSILON) || supportAlreadyIn)
			{
				sol.p1 = p1Temp;
				sol.p2=  p2Temp;

				//sol.indexPoly1PointInSolution = simplex1.indicesPoly[0];
				//sol.indexPoly1PointInSolution = simplex1.indicesPoly[simpSol.numVerticesUsed-1];
				//sol.indexPoly2PointInSolution = simplex2.indicesPoly[0];
				//sol.indexPoly2PointInSolution = simplex2.indicesPoly[simpSol.numVerticesUsed-1];

				sol.distance = d.length();
				return;
			}

			//internal point: compenetration
			if(simpSol.numVerticesUsed == Simplex.LARGER_2D_SIMPLEX)
			{
				sol.p1 = p1Temp;
				sol.p2 = p2Temp;

				//sol.indexPoly1PointInSolution = simplex1.indicesPoly[0];
				//sol.indexPoly1PointInSolution = simplex1.indicesPoly[simpSol.numVerticesUsed-1];
				//sol.indexPoly2PointInSolution = simplex2.indicesPoly[0];
				//sol.indexPoly2PointInSolution = simplex2.indicesPoly[simpSol.numVerticesUsed-1];

				//TODO distanza negativa tale che  d(Ki,Kj) = -min{|z| : z appartien a boundary di K}
				sol.distance = 0;
				return;
			}

			/*if(supportAlreadyIn)
			{
				sol.p1 = p1Temp;
				sol.p2 = p2Temp;
				sol.distance = d.length();
				return;
			}*/

			if(simplexZ.currentDim < Simplex.LARGER_2D_SIMPLEX)
			{
				simplex1.vs[simpSol.numVerticesUsed] = poly1.getVertex(index1);
				PolyPointWitness witness1 = new PolyPointWitness();
				witness1.index = index1;
				simplex1.witnesses[simpSol.numVerticesUsed]/*indicesPoly[simpSol.numVerticesUsed]*/ = witness1;
				simplex1.currentDim++;

				simplex2.vs[simpSol.numVerticesUsed] = poly2.getVertex(index2);
				PolyPointWitness witness2 = new PolyPointWitness();
				witness2.index = index2;
				simplex2.witnesses[simpSol.numVerticesUsed]/*indicesPoly[simpSol.numVerticesUsed]*/ = witness2;
				simplex2.currentDim++;

				simplexZ.vs[simpSol.numVerticesUsed] = zSupport;
				simplexZ.currentDim++;
			}

		}

		throw new NotConvergingException();
	}


	private static Vec2 getPointSolution(Simplex simplex12, Simplex simplexZ, SimplexSolution npS)
	{
		Vec2 result = new Vec2();
		result.setZero();
		for(int i=0; i<npS.numVerticesUsed;i++)
		{
			int j;
			for( j=0; !simplexZ.vs[j].equals(npS.verticesInSolution[i]);j++);

			result.addLocal(simplex12.vs[j].mul(npS.coefficients[i]));

		}

		return result;
	}
}
